#! /usr/bin/env python
#codeing=utf-8

import rospy
from gazebo_msgs.srv import *
from tqdm import tqdm
import pickle
import numpy as np

filename = './src/plan_control/goals/spider_0.pkl'
get_state_service = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)

nums = 18
goals = []

model = GetModelStateRequest()
model.model_name = 'unit_box' 
objstate = get_state_service(model)
state = [objstate.pose.position.x, objstate.pose.position.y]
goals.append(state)
for i in tqdm(range(nums)):
  model = GetModelStateRequest()
  model.model_name = 'unit_box_'+str(i) 
  objstate = get_state_service(model)
  state = [objstate.pose.position.x, objstate.pose.position.y]
  goals.append(state)
goals = np.array(goals)
print(goals)
# pickle.dump(goals, open(filename, 'wb'))
print("finish!!!")